#include "identification_headers.h"

using namespace NEWMAT;
using namespace DATMO;
using namespace pcl;

class Identification {
public:
        Identification();
        void Identification::Detected_Objects_Callback(const datmo3::DetectedObjects_Msg::ConstPtr& msg);

        ros::NodeHandle n;
        ros::Publisher detected_objects_pub;
        string trial_folder_name;
        RobotState RobotStateData;
protected:
        int iteration;
        int it_identification_msg;
        IdentificationData ID;
};

//Class Initialization
Identification::Identification(){
  iteration = 1;
  it_identification_msg = 1;
  detected_objects_pub = n.advertise<datmo3::DetectedObjects_Msg>("datmo/identifiedobjects",1000);

}

void Identification::Detected_Objects_Callback(const datmo3::DetectedObjects_Msg::ConstPtr& msg)
{
  cl_DynamicObjects DynObjects = cl_Conversions::DetObjRosMsg2DetectedObjects(msg);

  ID.DynamicObjects = cl_ObjectIdentification::ObjectIdentification(DynObjects,ID.DynamicObjects,ros::Time::now());
  float aux1 = 0;
  ID.DynamicObjects_kalman = cl_KalmanObjectIdentification::KalmanFilterMain(DynObjects,aux1,aux1);

}


int main(int argc, char **argv)
{
  ros::init(argc,argv,"identification");
  ROS_INFO("PFUNCIONA!");
  Identification *controller = new Identification();


  //Time string for saving purposes
  time_t now = time(NULL);
  struct tm* now_time = gmtime(&now);
  std::stringstream time_str;
  time_str << now_time->tm_year+1900 <<"_" << now_time->tm_mon+1 <<"_";
  time_str << now_time->tm_mday <<"_" << now_time->tm_hour <<"_";
  time_str << now_time->tm_min <<"_" << now_time->tm_sec <<"_";
  //folder creation
  std::stringstream folder_name;
  folder_name << "./trial_" << time_str.str();
  controller->trial_folder_name = folder_name.str();
  string folder_string = folder_name.str();
  mkdir(folder_string.c_str(),S_IRWXU);

  ROS_INFO("FUNCIONA2222!");
  controller->n.getParam("Robot_ID",controller->RobotStateData.RobotID);
  controller->n.getParam("Robot_name",controller->RobotStateData.RobotName);
  ROS_INFO("PFUNCIONA!3333");

  //ROS Subscribers Creation

  ros::Subscriber detected_objects_sub = controller->n.subscribe("datmo/detectedobjects",1000,&Identification::Detected_Objects_Callback,controller);
  ros::Rate loop_rate(30);
  while(controller->n.ok())
  {
    controller->detected_objects_pub.publish(controller->ID.DynamicObjects);

    loop_rate.sleep();
  }

 // ros::spin();
  delete controller;


  return 0;


}
